/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : ekf_uwb_ins.hpp
  * Description        : 
  ******************************************************************************
  * Function           :
  *
  ******************************************************************************
  * Author             : Zhiping Fu
  * Creation Date      : Nov 25, 2020
  ******************************************************************************
  */
/* USER CODE END Header */

#ifndef __EKF_UWB_INS_HPP
#define __EKF_UWB_INS_HPP

#include "userlib/userlib.hpp"
#include "system/system.hpp"


#ifdef __cplusplus

#include <Eigen>

using namespace std;
using namespace Eigen;

class EKFUWB
{
public:
	EKFUWB(){}
	EKFUWB(char *n) : name(n){}
	~EKFUWB(){}
	void EkfUwb_Init();
	void EkfUwb_Update();

private:
	char *name;
	sTIM Tim;
	bool Update;
	uint32_t startTimer;
	uint32_t stopTimer;
	uint32_t  executionTime_us;
	uint32_t startTimerLast;
	uint32_t cycleTime_us;


	//矩阵运算变量定义（EKF6）
	Matrix<float,2,6> H;		//灵敏度矩阵
	Matrix<float,6,2> K;		//kalman gain
	Matrix<float,6,6> P_p;		//协方差矩阵预测
	Matrix<float,6,6> P_k;		//协方差矩阵更新
	Matrix<float,2,2> R;		//测量噪声
	Matrix<float,4,4> Q;		//过程噪声
	Vector2f y;					//测量值
	Vector2f y_p;				//预测测量
	Matrix<float,6,1> X_p;		//状态预测
	Matrix<float,6,1> X_k;		//状态更新
	Matrix<float,6,1> X_k_error;		//状态误差更新
	Matrix<float,6,6> F;
	Matrix<float,6,2> B;
	Matrix<float,6,4> G;

	Matrix<float,3,3> Cb2n;	//机体坐标系到NED的旋转矩阵

	Vector3f a_k;			//机体坐标系下的加速度
	Vector2f u_k;			//NED坐标系下XY轴加速度
	Vector3f accNED;
	float dt;

	sensor_acc_msg acc;
	uwb_pos_msg uwbPos;
	Cb2n_msg MatCb2n;
	ekf_uwb_pos_vel_msg ekf_uwb_pos_vel;

};

#endif


#endif


/************************ (C) COPYRIGHT Longmen Drone Team *****END OF FILE****/
